﻿#include <stdio.h>
#include <string.h>
#include <math.h>
#include <vec
#include "userstruct.h"

void   clac07root();//计算ECEF数据生成文件
int    GetReadCGS2000Para(CGS2000_PAR  *mCGS2000Para);
double Frac(double x);//Fractional part of a number (y=x-[x])
double Modulo(double x, double y);//x mod y
double EccAnom (double M, double e);
double delt_WeekSecT(WeekSecTimeA *_t1,WeekSecTimeA *_t2);//计算两个WeekSecTimeA之间的秒数差
void   WeekSecTimeA_validity(WeekSecTimeA *_tm);/*将时间表达形式规范化*/

//<-----------全局变量-----------------
X32          mBrdEPH;          //星历存储变量， 注意：22参数仅用18参数
CGS2000_PAR  mCGS2000Para;     //CGS2000坐标系参数定义
double       FitArcLength=3600;//总拟合弧长3600秒--60分钟
//-----------全局变量----------------->

int main(int argc, char *argv[])
{
    printf("hello c world.\n");
    memset(&mBrdEPH, 0, sizeof(mBrdEPH)); //初始化星历参数初值全部为0

    //2-----下面计算18根数生成位置文件
    clac07root();
    printf("calc finished.\n");

    return 0;
}

//计算ECEF数据生成文件
void clac07root()
{
    /*
    FitArc:          20  min
    ** 0 869  360604.000000
    a:      7.547836635268E+06
    e:      7.128103573562E-04
    i0:     1.509700698009E+00
    OMEGA0: 5.536003515205E+00
    w:      2.967049432946E+00
    M0:     3.894134449615E+00
    doti:   -4.900616618265E-08
    Toe:   3.606039999998E+05
    */

    /*时间变量，用于标识需要计算卫星状态信息*/
    WeekSecTimeA       GoalTime;
    char               OutStrChars[200];
    /*用于保存CGS2000的有关参数的定义常量*/
    double  GoalSatPos[6];         //保存目标时刻卫星的位置
    double  CurEphPara[7];     //使用的卫星广播星历，矢量表达
    int   i,nn; //临时变量
    FILE *hRstFile;

    /*时间变量：周＋秒，保存当前星历的参考时间*/
    WeekSecTimeA  CurEphRefTime;

    // Variables
    double  a,e,Parai,Omega,M,M0,n,p,omega;
    double  E,Omegak,tk;
    double  Vk,fik;
    double  uk,rk,ik,xk,yk,tempf;
    double  sinE,cosE,sqrt1_e2;

    GetReadCGS2000Para(&mCGS2000Para );
    if ( (hRstFile = fopen( "ECEF_EPH7Para_POSVEL.rst","wt") ) == NULL )
       return ;

    mBrdEPH.RNSSbrd_arry[0].ref_epoch_week = 869;
    mBrdEPH.RNSSbrd_arry[0].ref_epoch_sow  = 360604.0;
    mBrdEPH.RNSSbrd_arry[0].a     = 7.547836635268E+06;
    mBrdEPH.RNSSbrd_arry[0].e     = 7.128103573562E-04;
    mBrdEPH.RNSSbrd_arry[0].i0    = 1.509700698009E+00;
    mBrdEPH.RNSSbrd_arry[0].OMEGA0 = 5.536003515205E+00;
    mBrdEPH.RNSSbrd_arry[0].omega  = 2.967049432946E+00;
    mBrdEPH.RNSSbrd_arry[0].M0     = 3.894134449615E+00;
    mBrdEPH.RNSSbrd_arry[0].idot   = -4.900616618265E-08;

    //----------------!!!!!!!!!修改12rootcal时注意业要把这句去掉，否则默认mBrdEPH.Ngp=0，直接退出-------
    //如果没有有效的星历---
    //if (mBrdEPH.Ngp <= 0)return ;

    //临时设置的计算卫星位置的时间起点
    GoalTime.Week = mBrdEPH.RNSSbrd_arry[0].ref_epoch_week;
    GoalTime.Secs = mBrdEPH.RNSSbrd_arry[0].ref_epoch_sow-10*60;

    /*当前星历参考时间的周、秒*/
    CurEphRefTime.Week =  mBrdEPH.RNSSbrd_arry[0].ref_epoch_week;
    CurEphRefTime.Secs =  mBrdEPH.RNSSbrd_arry[0].ref_epoch_sow;
    WeekSecTimeA_validity(&CurEphRefTime);//CurEphRefTime.validity();

    i = 0;
    CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[0].a;
    CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[0].e;
    CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[0].i0;
    CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[0].OMEGA0;
    CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[0].omega;
    CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[0].M0;
    CurEphPara[i++] = mBrdEPH.RNSSbrd_arry[0].idot;

    //为了解算卫星速度专用的变量
    double xkRate,ykRate,OmegakRate,rkRate,ukRate,ikRate,ekRate,fikRate;
    double n0,A0,Ek;
    //计算21个点
    for(nn=0; nn<=20; nn++ )   //60秒计算一个点
    {
        tk = delt_WeekSecT(&GoalTime,&CurEphRefTime);//tk = GoalTime - CurEphRefTime;//计算与参考时刻的时间差tk = t - toe
        a = CurEphPara[0];
        e = CurEphPara[1] ;
        Parai  = CurEphPara[2]+CurEphPara[6]*tk;//ik

        Omegak = CurEphPara[3];//
        omega  = CurEphPara[4];
        n = sqrt(mCGS2000Para.GM/(a*a*a)); //-----平均角速度n
        M = CurEphPara[5] + n*tk;
        p  = a*( 1.0-e*e );
        E  = EccAnom(M,e);
        Vk = 2.0*atan( sqrt((1+e)/(1-e))*tan(E/2.0) );  // old from clc
        fik = Vk + omega;
        uk = fik;
        rk = a*(1.0-e*cos(E));
        ik = Parai;
        xk = rk*cos(uk);
        yk = rk*sin(uk);
        Omegak -= mCGS2000Para.omega_earth*tk;

        //计算卫星在地心固定坐标系中的直角坐标
        GoalSatPos[0] = xk*cos( Omegak ) - yk *cos( ik ) * sin( Omegak );
        GoalSatPos[1] = xk*sin( Omegak ) + yk *cos( ik ) * cos( Omegak );
        GoalSatPos[2] = yk*sin( ik );
        //<------------------------------------------------------
        A0 =  a;
        Ek = E;
        n0 = sqrt (mCGS2000Para.GM/(A0*A0*A0)); //-----平均角速度n

        //double r = A0*(1-e*cos(E));//卫星与地心的距离为r=a(1-e*cos(E))
        //计算j2000的速度
        double l1,l2,n1,n2,m1,m2;
        l1 = cos(omega)*cos(Omegak)-sin(omega)*cos(Parai)*sin(Omegak);
        l1 = cos(omega)*cos(Omegak)-sin(omega)*cos(Parai)*sin(Omegak);



        ekRate = n0/(1.0-e*cos(Ek));

        rkRate = A0 * e *sin(Ek) *ekRate;
        ikRate = mBrdEPH.RNSSbrd_arry[0].idot;
        fikRate = sqrt( 1.0-e*e ) * ekRate /( 1.0-e*cos(Ek) );
        ukRate = fikRate;
        xkRate = rkRate * cos(uk) - rk * sin(uk) * ukRate;
        ykRate = rkRate * sin(uk) + rk * cos(uk) * ukRate;

        //地固系中的坐标、速度求解
        //OmegakRate = mBrdEPH.RNSSbrd_arry[0].idot - mCGS2000Para.omega_earth;
        /*12.当前时刻卫星在J2000中的三维速度*/
//        GoalSatPos[3] = ((((xkRate * cos(Omegak))
//                  - ((ykRate * sin(Omegak)) * cos(ik)))
//                 + (((yk * sin(Omegak)) * sin(ik)) * ikRate))
//                - ((xk * sin(Omegak)) * OmegakRate))
//                - (((yk * cos(Omegak)) * cos(ik)) * OmegakRate);
//        GoalSatPos[4] = ((((xkRate * sin(Omegak))
//                  + ((ykRate * cos(Omegak)) * cos(ik)))
//                 - (((yk * cos(Omegak)) * sin(ik))* ikRate))
//                + ((xk * cos(Omegak)) * OmegakRate))
//                - (((yk * sin(Omegak)) * cos(ik)) * OmegakRate);
//        GoalSatPos[5] = ykRate * sin(ik) + yk*cos(ik)*ikRate;
        //------------------------------------------------------>

        //sprintf( OutStrChars,"%12.5E %03d  %7.2lf  %12.3lf  %12.3lf %12.3lf",
        sprintf( OutStrChars,"%09d   %06d   %7.2lf   %12.3lf   %12.3lf   %12.3lf  %09.4lf  %09.4lf %09.4lf",
                 nn*60, GoalTime.Week, GoalTime.Secs, GoalSatPos[0], GoalSatPos[1],GoalSatPos[2], GoalSatPos[3], GoalSatPos[4],GoalSatPos[5]);
        fprintf(  hRstFile, " %s \n", OutStrChars );
        GoalTime.Secs += 60;  //一分钟算一个结果
        WeekSecTimeA_validity(&GoalTime);//GoalTime.validity();
   }

   if( hRstFile )fclose( hRstFile );
}

struct PosVel
{
    vector<DateTime> dateTime;
    vector<double> x;
    vector<double> y;
    vector<double> z;
    vector<double> vx;
    vector<double> vy;
    vector<double> vz;
};

PosVel Lagrange_posvel(vector<CDateTime> time,vector<double> x,vector<double> y,vector<double> z,vector<double> vx,vector<double> vy,vector<double> vz)
{
    PosVel posvel;
    int n=time.size();
    vector<double> t;
    for(int i=0;i<time.size();i++)
    {
        t.push_back(i*60);
    }
    double numerator_cofficient;      //用来记录插值分子的乘积结果
    double denominator_coefficient;      //用来记录插值分母乘积的结果
    double input_x=(n-1)*60;      //需要输入的x的值
    for(int i=0;i<n;i++)
    {
        denominator_coefficient=1;
        for (int j=0;j<n;j++)
        {
            if (i==j)
                continue;
        }
        for (int j=0;j<n;j++)
        {
            if (i==j)
                continue;
            denominator_coefficient*=(t[i]-t[j]);
        }
    }
    for(int kk=0;kk<=input_x;kk++)
    {
        double resultT=0,resultX=0,resultY=0,resultZ=0,resultVX=0,resultVY=0,resultVZ=0;      //用来记录插值结果
        for (int i=0;i<n;i++)
        {
            numerator_cofficient=1;
            denominator_coefficient=1;
            for (int j=0;j<n;j++)
            {
                if (i==j)
                    continue;
                numerator_cofficient*=(kk-t[j]);
            }
            for (int j=0;j<n;j++)
            {
                if (i==j)
                    continue;
                denominator_coefficient*=(t[i]-t[j]);
            }
            resultX+=(x[i]*numerator_cofficient/denominator_coefficient);
            resultY+=(y[i]*numerator_cofficient/denominator_coefficient);
            resultZ+=(z[i]*numerator_cofficient/denominator_coefficient);
            resultVX+=(vx[i]*numerator_cofficient/denominator_coefficient);
            resultVY+=(vy[i]*numerator_cofficient/denominator_coefficient);
            resultVZ+=(vz[i]*numerator_cofficient/denominator_coefficient);
            resultT=kk;
        }
        CDateTime outTime=time[0]+resultT;
        int year,month,day,hour,min;double sec;
        GetCalendar(outTime.GetMJD(),year,month,day,hour,min,sec);
        DateTime inTime;
        inTime.year=year;inTime.month=month;inTime.day=day;inTime.hour=hour;inTime.minute=min;inTime.second=sec;
        posvel.dateTime.push_back(inTime);
        posvel.x.push_back(resultX);
        posvel.y.push_back(resultY);
        posvel.z.push_back(resultZ);
        posvel.vx.push_back(resultVX);
        posvel.vy.push_back(resultVY);
        posvel.vz.push_back(resultVZ);
//    qDebug()<<inTime.year<<inTime.month<<inTime.day<<inTime.hour<<inTime.minute<<inTime.second
//           <<resultX<<resultY<<resultZ<<resultVX<<resultVY<<resultVZ;

}
    return posvel;
}




